#include "mpu6050.h"
#include <stdint.h>
#include "AirMouse.h"
#include "CH58xBLE_LIB.h"
#include "AccelerateCalculus.h"
#include "WinyunqConfigure.h"

union
{
    struct
    {
        volatile struct
        {
            uint8_t left : 1;  //|–bit0:1表示left按下
            uint8_t right : 1; //|–bit1:1表示右键按下
            uint8_t mid : 1;   //|–bit2:1表示mid按下
            uint8_t LEDOn : 1; //|–bit3:恒为1
            uint8_t fleft : 1; //|–bit4:X 坐标变化的符号位，1表示负数，即鼠标fleft移动?????WTF
            uint8_t fdown : 1; //|–bit5:Y 坐标变化的符号位，1表示负数，即鼠标fdown移动?????WTF
            uint8_t x : 1;     //|–bit6:1 表示 X 坐标的变化量超出－256 ~ 255的范围，0表示没有溢出
            uint8_t y : 1;     //|–bit7:1 表示 Y 坐标的变化量超出－256 ~ 255的范围,0表示没有溢出
        } situation;           //第1个字节:详情如上
        int8_t x;             //第2个字节:X轴位移量
        int8_t y;             //第3个字节:Y轴位移量
        int8_t z;             //第4个字节:Z轴位移量,即滚轮
    } details;
    uint8_t buffer[4];
    int32_t data; // 32位CPU一次性清空
} MouseData;      //上报数据表,4字节

float NowAccX, NowAccY, NowAccZ;

struct Vector3
{
    int32_t x;
    int32_t y;
    int32_t z;
}location,LastLocation,speed,accelerate,MinAccelerate,NowAngle;
/// 角速度版所用计算变量
Vector3_16 AngleSpeed,Accelerate,StaticAccelerate;

uint32_t LastTime;

tmosTaskID MouseCalculusTaskID;

uint8_t AccelerateIsInReading;


unsigned char isInUsing = 0;
/**
 * @brief           加速度积分进程                                      
 *  @details        不断进行加速度积分的进程，直到使用键松开
 * 
 * @param           参数名称:【task_id】      数据类型:             参数说明
 * @param           参数名称:【events】       数据类型:             参数说明
 * 
 * @return          uint16类型            返回值介绍                                                         
 *  @retval         【返回值分段则必选】值或范围      说明
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-11-02
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-11-02
 * @version         1.0.0
 */
uint16_t MouseCalculusTask( uint8_t task_id, uint16_t events ){
    AccelerateCalculus();
  if(isInUsing){
      printf("keep");
       tmos_start_task(MouseCalculusTaskID,0,10);
  }
   
  return 0;
}
/*********************************************************************
 * @fn      MouseInit
 *
 * @brief   对鼠标所需要用到的外设初始化
 *
 * @param   无
 *
 * @return  无
 */
void MouseInit()
{
    MouseConfigure.details.MoveType=0;//测试用，正确用法应该是读取flash
    MouseConfigure.details.xylist=175;
     MouseConfigure.details.report=150;
     ReadType=MPU6050Accelerate;
#if ( LeftTouchGroup == 'A')
    GPIOA_ModeCfg(LeftTouch, GPIO_ModeIN_PU);  //左键初始化
#elif ( LeftTouchGroup == 'B')
    GPIOB_ModeCfg(LeftTouch, GPIO_ModeIN_PU);  //左键初始化
#endif
#if (RightTouchGroup=='A')
    GPIOA_ModeCfg(RightTouch, GPIO_ModeIN_PU);  //右键初始化
#elif (RightTouchGroup=='B')
    GPIOB_ModeCfg(RightTouch, GPIO_ModeIN_PU);  //右键初始化
#endif
#if (UsingKeyTouchGroup=='A')
    GPIOA_ModeCfg(UsingKeyTouch, GPIO_ModeIN_PU);  //使用键初始化
#elif (UsingKeyTouchGroup=='B')
    GPIOB_ModeCfg(UsingKeyTouch, GPIO_ModeIN_PU);  //使用键初始化
#endif    
    MoveDeviceInit(); //初始化MPU6050
    MinAccelerate.x = 75;
    MinAccelerate.y = 75;
    MinAccelerate.z = 75;
    MouseCalculusTaskID=TMOS_ProcessEventRegister( MouseCalculusTask );
}
/**
 * @brief           加速度积分                                      
 *  @details        进行一次加速度积分
 * 
 * @param           参数名称:【task_id】      数据类型:             参数说明
 * @param           参数名称:【events】       数据类型:             参数说明
 * 
 * @return          uint16类型            返回值介绍                                                         
 *  @retval         【返回值分段则必选】值或范围      说明
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-11-03
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-11-03
 * @version         1.0.0
 */
//__HIGH_CODE
void AccelerateCalculus(){
    printf("\nrun ADD\n");
    uint32_t time;
    uint32_t Nowtime=TMOS_GetSystemClock();
    RISCVReadMPU(&Accelerate,0x3B);//读取当前速度
    time=Nowtime-LastTime;
    LastTime=Nowtime;
    speed.x+=(Accelerate.x-StaticAccelerate.x)*time;
    speed.y+=(Accelerate.y-StaticAccelerate.y)*time;
    speed.z+=(Accelerate.z-StaticAccelerate.z)*time;
    location.x += speed.x * time; //计算位置积分
    location.y += speed.y * time;
    location.z += speed.z * time;
    if(AccelerateIsInReading){// 如果直接读取Location，可能会出现读取过程中出现系统调度，导致最新的Location更新，而系统将上一次的Location作为最新的Location处理。
    // 但是即使出现数据过时的问题，其影响可能微乎其微。而像这样写，虽然性能会稍差，但是这样写对性能的影响也是微乎其微。因为系统的效果取决于加速度积分的频率，也就是MouseCalculusTask的执行率
        MouseData.details.x =(location.x - LastLocation.x) / MouseConfigure.details.xylist;
        MouseData.details.y =(location.x - LastLocation.x) / MouseConfigure.details.xylist;
        LastLocation.x += MouseData.details.x * MouseConfigure.details.xylist; //保留数据溢出误差
        LastLocation.y += MouseData.details.y * MouseConfigure.details.xylist; //保留数据溢出误差
        LastLocation.z = location.z;
        AccelerateIsInReading=0;
    }
}
/*********************************************************************
 * @fn      ReadKeyBoardState
 *
 * @brief   读取鼠标左右键状态，并且返回是否触发移动键
 *
 * @param   无
 *
 * @return  False -鼠标未移动
 *          True  -鼠标在移动
 */
unsigned char ReadKeyBoardState()
{
#if (LeftTouchGroup=='A')
    if (!GPIOA_ReadPortPin(LeftTouch))
#elif (LeftTouchGroup=='B')
    if (!GPIOB_ReadPortPin(LeftTouch))
#endif  
    {
        MouseData.details.situation.left = 1;
    } //读取左键按钮
#if (RightTouchGroup=='A')
    if (!GPIOA_ReadPortPin(RightTouch))
#elif (RightTouchGroup=='B')
    if (!GPIOB_ReadPortPin(RightTouch))
#endif
    {
        MouseData.details.situation.right = 1;
    }         //读取右键按钮
#if (UsingKeyTouchGroup=='A')
    return ! GPIOA_ReadPortPin(UsingKeyTouch);//返回使用键状态
#elif (UsingKeyTouchGroup=='B')
    return ! GPIOB_ReadPortPin(UsingKeyTouch);//返回使用键状态
#endif
}
extern void Kalman(int32_t time);
double LastKalmanX,LastKalmanY;
/*********************************************************************
 * @fn      ReadMoveDate
 *
 * @brief   读取鼠标移动值
 *              该版本不完善，完善版应该引入角速度，对坐标实时变换积分
 *
 * @param   无
 *
 * @return  无
 */
void ReadMoveDate(int32_t time)
{
    
    /*ReadAccelerate(&accelerate);
    // MPU_Get_Accelerometer(&accelerate.x,&accelerate.y,&accelerate.z);//测量加速度
    // printf("ax %lld    ,ay %lld   , az %lld",accelerate.x,accelerate.y,accelerate.z);
    printf("\n 原始数据：ax %d    ,ay %d   , az %d\n", accelerate.x, accelerate.y, accelerate.z);
    int32_t AX = accelerate.x - StaticAccelerate.x; //抵消x方向上的重力加速度
    if ((AX < MinAccelerate.x) && (AX > -MinAccelerate.x))
        AX = 0;
    int32_t AY = accelerate.y - StaticAccelerate.y; //抵消y方向上的重力加速度
    if ((AX < MinAccelerate.y) && (AX > -MinAccelerate.y))
        AY = 0;
    int32_t AZ = accelerate.z - StaticAccelerate.z; //抵消z方向上的重力加速度
    if ((AX < MinAccelerate.z) && (AX > -MinAccelerate.z))
        AZ = 0;
    printf("\n 有效加速度: %d %d %d", (int32_t)AX, (int32_t)AY, (int32_t)AZ);
    speed.x += AX * time; //计算速度积分
    speed.y += AY * time;
    speed.z += AZ * time;
    printf("\n 当前速度: %d %d %d", (int32_t)speed.x, (int32_t)speed.y, (int32_t)speed.z);
    location.x += speed.x * time; //计算位置积分
    location.y += speed.y * time;
    location.z += speed.z * time;
    int16_t x = (location.x - LastLocation.x) / MouseConfigure.details.xylist;
    MouseData.details.x = x;                            //计算经过系数转换后，实际要上报的位移X
    int16_t y = (location.y - LastLocation.y) / MouseConfigure.details.xylist; //计算经过系数转换后，实际要上报的位移Y
    MouseData.details.y = y;
    LastLocation.x += x * MouseConfigure.details.xylist; //保留数据溢出误差
    LastLocation.y += y * MouseConfigure.details.xylist; //保留数据溢出误差
    LastLocation.z = location.z;
    printf("\n X:%d Y:%d", x, y);*/
#define level 7
#define list 15
int32_t AX=0,AY=0,AZ=0;
    for(int i=0;i<level;i++){
    //RISCVReadMPU(&Accelerate,0x3B);//读取当前速度
    LoadAccelerate();
    GetAccelerate(&Accelerate);
    printf("\n当前加速度X:%d,Y:%d,Z:%d",Accelerate.x-StaticAccelerate.x,Accelerate.y-StaticAccelerate.y,Accelerate.z-StaticAccelerate.z);
         AX += (Accelerate.x - StaticAccelerate.x)/list; //抵消x方向上的重力加速度
    if ((AX < MinAccelerate.x) && (AX > -MinAccelerate.x))
        AX = 0;
     AY += (Accelerate.y - StaticAccelerate.y)/list; //抵消y方向上的重力加速度
    if ((AX < MinAccelerate.y) && (AX > -MinAccelerate.y))
        AY = 0;
     AZ +=( Accelerate.z - StaticAccelerate.z)/list; //抵消z方向上的重力加速度
    if ((AX < MinAccelerate.z) && (AX > -MinAccelerate.z))
        AZ = 0;
    speed.x+=AX;//(Accelerate.x-StaticAccelerate.x)*time;
    speed.y+=AY;(Accelerate.y-StaticAccelerate.y)*time;
    speed.z+=AZ;(Accelerate.z-StaticAccelerate.z)*time;
    }
    speed.x/=level;
    speed.y/=level;
    speed.z/=level;
    location.x += speed.x * time; //计算位置积分
    location.y += speed.y * time;
    location.z += speed.z * time;
    printf("\n当前速度X:%d,Y:%d,Z:%d",speed.x,speed.y,speed.z);
    int16_t x = (location.x - LastLocation.x) ;//make/ MouseConfigure.details.xylist;
    MouseData.details.x = x;                            //计算经过系数转换后，实际要上报的位移X
    int16_t y = (location.y - LastLocation.y) ;/// MouseConfigure.details.xylist; //计算经过系数转换后，实际要上报的位移Y
    MouseData.details.y = y;
    printf("\n位移X:%d Y:%d", x, y);
    LastLocation.x += x * MouseConfigure.details.xylist; //保留数据溢出误差
    LastLocation.y += y * MouseConfigure.details.xylist; //保留数据溢出误差
    LastLocation.z = location.z;
    /*
    AccelerateIsInReading=1;//告诉积分进程，下一次积分完成后需要计算一次数据包
    //while(AccelerateIsInReading)printf("wait");//等待积分进程完成填写数据包任务
    */
}

/*********************************************************************
 * @fn      ReadMoveDateAngle
 *
 * @brief   读取鼠标移动值(角速度版)
 *
 * @param   无
 *
 * @return  无
 */
void ReadMoveDateAngle()
{
    LoadAccelerate();
    GetAccelerate(&AngleSpeed);//读取角速度
    AngleSpeed.x-=StaticAccelerate.x;
    AngleSpeed.y-=StaticAccelerate.y;
    AngleSpeed.z-=StaticAccelerate.z;
    printf("x:%d,y:%d,z:%d\n",AngleSpeed.x,AngleSpeed.y,AngleSpeed.z);
    MouseData.details.x = -(AngleSpeed.z) / MouseConfigure.details.xylist;//计算经过系数转换后，实际要上报的位移X
    MouseData.details.y = (AngleSpeed.x) / MouseConfigure.details.xylist;//计算经过系数转换后，实际要上报的位移Y,
}
    extern struct{
    double Accel_X;
    double Accel_Y;
    double Accel_Z;
    double Gyro_X;
    double Gyro_Y;
    double Gyro_Z;
    double Angle_X;
    double Angle_Y;
    double Angle_Z;
    double Temp;
    double KalmanAngleX;
    double KalmanAngleY;
    }IMU_Data;
/**
 * @brief           角速度移动Kalman滤波版                                      
 *  @details        使用Kalman对角速度进行滤波
 * 
 * 
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2023-01-20
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2023-01-20
 * @version         1.0.0
 */
void ReadKalmanMoveDateAngle(uint32_t time)
{
    Kalman(time);
    /// 读取MPU滤波后的值
    MouseData.details.x = IMU_Data.KalmanAngleX-LastKalmanX; 
    MouseData.details.y = IMU_Data.KalmanAngleY-LastKalmanY;
    /// @brief 保留精度误差
    LastKalmanX+=MouseData.details.x;
    LastKalmanY+=MouseData.details.y;
    /// 调转X的方向
    MouseData.details.x=-MouseData.details.x;
    printf("\n位移X:%d Y:%d", MouseData.details.x, MouseData.details.y);
    return; 
}
/*********************************************************************
 * @fn      BuildSituation
 *
 * @brief   以当前状态为静止参考系，构建静态参考系
 *
 * @param   无
 *
 * @return  无
 */
void BuildSituation()
{
    location.x = location.y = location.z = 0;             //重置位移积分
    LastLocation.x = LastLocation.y = LastLocation.z = 0; //重置上报位移积分
    speed.x = speed.y = speed.z = 0;                      //重置速度
    
    InitAccelerate();
    GetAccelerate(&StaticAccelerate);
    // MPU_Get_Accelerometer(&StaticAccelerate.x,&StaticAccelerate.y,&StaticAccelerate.z);//将当前加速度设置为重力加速度参考
    // printf("\n 重力加速度: %d %d %d \n",(int32_t)StaticAccelerate.x,(int32_t)StaticAccelerate.y,(int32_t)StaticAccelerate.z);
    MinAccelerate.x = MinAccelerate.y = MinAccelerate.z = 0;
    // printf("\n 忽略性质的加速度: %d %d %d \n",(int32_t)MinAccelerate.x,(int32_t)MinAccelerate.y,(int32_t)MinAccelerate.z);
    LastTime=GetSysClock();
    /// @brief 清除之前Kalman预测的结果
    Kalman(32768);
    /*while((LastKalmanX==IMU_Data.KalmanAngleX)&&(LastKalmanY==IMU_Data.KalmanAngleY)){
    LastKalmanX=IMU_Data.KalmanAngleX;
    LastKalmanY=IMU_Data.KalmanAngleY;
    Kalman(32768);
    }/*
    while((LastKalmanX==IMU_Data.KalmanAngleX)&&(LastKalmanY==IMU_Data.KalmanAngleY)){
    LastKalmanX=IMU_Data.KalmanAngleX;
    LastKalmanY=IMU_Data.KalmanAngleY;
    Kalman(32768);
    }*/
    LastKalmanX=IMU_Data.KalmanAngleX;
    LastKalmanY=IMU_Data.KalmanAngleY;
    //tmos_start_task(MouseCalculusTaskID,0,10);
}
/*********************************************************************
 * @fn      MouseEvent
 *
 * @brief   鼠标处理事件
 *
 * @param   无
 *
 * @return  无
 */
void MouseEvent(){
    MouseData.data=0;
        if(ReadKeyBoardState()){//读取左右键状态并判断是否需处于移动状态
            if(isInUsing){
                if( ReadType==MPU6050Angle)
                    ReadMoveDateAngle();//如果是移动状态，且不是从静态进入移动态，则计算位移
                else {
                    uint32_t Nowtime=TMOS_GetSystemClock();
                    ReadKalmanMoveDateAngle(Nowtime-LastTime);
                    LastTime=Nowtime;
                }
            }
            else{//否则，从静态进入移动态需要重构静态坐标系
                isInUsing=1;//记录已进入移动状态
                BuildSituation();//构建静态坐标系
            }
        }
        else isInUsing=0;//如果未使用，标记当前状态为静态*/
    HidDev_Report(0, 1, 4, MouseData.buffer);
}
